#!/usr/bin/env python3

import rospy
import numpy as np
import open3d as o3d
from sensor_msgs import point_cloud2
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Header

def main():
    rospy.init_node('pcd_to_elv_test', anonymous=True)

    # 从ROS参数服务器获取参数
    pcd_path = rospy.get_param('~pcd_path')
    point_cloud_topic = rospy.get_param('~point_cloud_topic')
    frame_id = rospy.get_param('~frame_id')

    clip_xmin = rospy.get_param('~clip_range_xmin')
    clip_xmax = rospy.get_param('~clip_range_xmax')
    clip_ymin = rospy.get_param('~clip_range_ymin')
    clip_ymax = rospy.get_param('~clip_range_ymax')

    # 读取PCD文件
    rospy.loginfo(f"Loading PCD file from: {pcd_path}")
    pcd = o3d.io.read_point_cloud(pcd_path)
    points = np.asarray(pcd.points)

    # 裁剪点云
    rospy.loginfo(f"Clipping point cloud to x: [{clip_xmin}, {clip_xmax}], y: [{clip_ymin}, {clip_ymax}]")
    mask = (
        (points[:, 0] >= clip_xmin) &
        (points[:, 0] <= clip_xmax) &
        (points[:, 1] >= clip_ymin) &
        (points[:, 1] <= clip_ymax)
    )
    filtered_points = points[mask]

    # 构建PointCloud2消息
    header = Header()
    header.frame_id = frame_id

    fields = [
        PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
        PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
        PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1)
    ]

    # 转换为Point32元组列表
    points_list = [tuple(point) for point in filtered_points]

    cloud_msg = point_cloud2.create_cloud(header, fields, points_list)

    # 创建发布者
    pub = rospy.Publisher(point_cloud_topic, PointCloud2, queue_size=10)

    rate = rospy.Rate(1)  # 每秒发布一次
    rospy.loginfo("Publishing point cloud...")

    while not rospy.is_shutdown():
        cloud_msg.header.stamp = rospy.Time.now()
        pub.publish(cloud_msg)
        # rospy.loginfo("Point cloud published.")
        rate.sleep()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass